#include <utility>
using namespace std;

#include <dalelib/graphics/skinnedmesh.h>

namespace dalelib
{



float SkinnedMesh::distanceToLineSegment(v3 &_Start, v3 &_End, v3 &_Point)
{
	const float Tbias = 0.05f, Tmin = 0.f+Tbias, Tmax = 1.f-Tbias;
	v3 v = _End-_Start;
	if(v.len()<0.0001f) return (_Point-_Start).len();
	v3 p = _Point-_Start;
	float t = v3::dot(v,p)/v3::dot(v,v);
	if(t<Tmin) t=Tmin; if(t>Tmax) t=Tmax;
	return (_Point-(_Start+v*t)).len();
}

typedef pair<int,float> SkinIndexWeight;
struct SortSkinIndexWeightFunctor { bool operator()(SkinIndexWeight a, SkinIndexWeight b) { return a.second > b.second; } }; // functor - class that defines operator()

void SkinnedMesh::init(Skeleton *_Skeleton)
{
	if(!_loaded) { cout << "you have to load a mesh before you can attach a skeleton" << endl; return; }
	_skeleton =_Skeleton;
}

void SkinnedMesh::init(Agent *_Agent)
{
	if(!_loaded) { cout << "you have to load a mesh before you can attach an agent" << endl; return; }
	_agent =_Agent;
}

void SkinnedMesh::draw()
{
	float stride = sizeof(Vertex);

	glEnableClientState( GL_VERTEX_ARRAY );
	glVertexPointer( 3, GL_FLOAT, sizeof(Vertex), &_vertex_array[0]._pos );

	glEnableClientState( GL_NORMAL_ARRAY );
	glNormalPointer( GL_FLOAT, sizeof(Vertex), &_vertex_array[0]._nrm );

	glEnableClientState( GL_COLOR_ARRAY );
	glColorPointer( 3, GL_FLOAT, sizeof(Vertex), &_vertex_array[0]._col );

	glClientActiveTextureARB(GL_TEXTURE0_ARB);
	glEnableClientState( GL_TEXTURE_COORD_ARRAY );
	glTexCoordPointer( 3, GL_FLOAT, sizeof(Vertex), &_vertex_array[0]._uv );

	glClientActiveTextureARB(GL_TEXTURE1_ARB);
	glEnableClientState( GL_TEXTURE_COORD_ARRAY );
	glTexCoordPointer( 4, GL_FLOAT, 0, &_bone_index_array[0] );

	glClientActiveTextureARB(GL_TEXTURE2_ARB);
	glEnableClientState( GL_TEXTURE_COORD_ARRAY );
	glTexCoordPointer( 4, GL_FLOAT, 0, &_bone_weight_array[0] );

	glDrawElements( GL_TRIANGLES, _index_array.size(), GL_UNSIGNED_INT, (GLvoid*)&_index_array[0] );

	glDisableClientState( GL_VERTEX_ARRAY );
	glDisableClientState( GL_NORMAL_ARRAY );
	glDisableClientState( GL_COLOR_ARRAY );
	glDisableClientState( GL_TEXTURE_COORD_ARRAY );
	glClientActiveTextureARB(GL_TEXTURE0_ARB);

}

void SkinnedMesh::createWeights(const int _BlurPasses)
{

//	Skeleton::BoneList& b = _skeleton->getBoneList();
	Agent::BoneList& b = _agent->getBoneList();

	_bone_index_array.resize(_vertex_array.size());
	_bone_weight_array.resize(_vertex_array.size());


	vector<vector<SkinIndexWeight > > temp_weights;
	temp_weights.resize(_vertex_array.size());

	// *** set weights to 1 for closest bone, otherwise 0 ***
	for(int i=0; i<_vertex_array.size(); ++i)
	{
		int closest_index = 0;
		float closest_value = 999999.f;

		temp_weights[i].resize(b.size());

		for(int j=0; j<b.size(); ++j)
		{
//			Bone* current_bone = b[j];
			AgentBone* current_bone = b[j];
			AgentBone* parent = current_bone->getParent();
			float d = 0.f;
			if( parent )
			{
//				v3 start = current_bone->getGlobalMat().transformPoint(v3(0,0,0));
//				v3 end = current_bone->getGlobalMat().transformPoint(current_bone->getOffset()*2);

				v3 start = current_bone->getGlobalStartPos();
				v3 end = current_bone->getGlobalEndPos();

				d = distanceToLineSegment(start,end,_vertex_array[i]._pos);

				if( closest_value>d )
				{
					closest_value = d;
//					closest_index = j;
					closest_index = parent->getID();
				}
			}
		}


		for(int j=0; j<b.size(); ++j)
		{
			float f = 0.f;
			if(j==closest_index) f = 1.f;
			temp_weights[i][j] = SkinIndexWeight(j,f);
		}
	}

	// *** blur weights across triangles ***
	if( _BlurPasses )
	{
		static const float BLUR_STRENGTH = 0.005f;
		cout << "Creating skinning weights. Please wait..." << endl;

		for(int iter=0; iter<_BlurPasses; ++iter)
		{
			// *** blur ***
			for(int i=0; i<_index_array.size(); i+=3)
			{
				float flow[3];
				for(int j=0; j<3; ++j) flow[j] = BLUR_STRENGTH/((_vertex_array[_index_array[i+((j+1)%3)]]._pos - _vertex_array[_index_array[i+j]]._pos).len()+0.01f);


				for(int k=0; k<b.size(); ++k)
				{
//	       			Bone* current_bone = b[k];
//	       			AgentBone* current_bone = b[k];
//					Bone* parent = current_bone->getParent();
//					if( parent )
					{
						for(int j=0; j<3; ++j)
						{
							float diff = (temp_weights[_index_array[i+((j+1)%3)]][k].second - temp_weights[_index_array[i+j]][k].second)*flow[j];
							temp_weights[_index_array[i+j]][k].second += diff;
							temp_weights[_index_array[i+((j+1)%3)]][k].second -= diff;
						}
					}
				}
			}

//*
			// *** equalise points sharing space (blur across seams) ***
			const float DISTANCE_THRESHOLD = 0.05f;
//			for(int a=0; a<2; ++a) // repeat a few times to make sure no cracks
			{
				for(int j=0; j<_vertex_array.size(); ++j)
				{
					for(int i=0; i<_vertex_array.size(); ++i)
					{
						if( i!=j )
						{
							float dist = (_vertex_array[i]._pos-_vertex_array[j]._pos).len();
							if(dist<DISTANCE_THRESHOLD)
							{
								for(int k=0; k<b.size(); ++k)
								{
									float av = (temp_weights[i][k].second+temp_weights[j][k].second)*0.5f;
									temp_weights[i][k].second = av;
									temp_weights[j][k].second = av;
								}
							}
						}
					}
				}
			}
//*/
		}
	}

	int max_weights = 4; if(max_weights>b.size()) max_weights = b.size();
	// *** sort and crop ***
	for(int i=0; i<_vertex_array.size(); ++i)
	{
		sort(temp_weights[i].begin(), temp_weights[i].end(), SortSkinIndexWeightFunctor() );
		temp_weights[i].erase(temp_weights[i].begin()+max_weights,temp_weights[i].end()); // only keep highest 4
		float sum = temp_weights[i][0].second + temp_weights[i][1].second + temp_weights[i][2].second + temp_weights[i][3].second; // normalise weights
		_bone_index_array[i] = v4(temp_weights[i][0].first, temp_weights[i][1].first, temp_weights[i][2].first, temp_weights[i][3].first);
		_bone_weight_array[i] = v4(temp_weights[i][0].second, temp_weights[i][1].second, temp_weights[i][2].second, temp_weights[i][3].second)/sum;
	}
}

void SkinnedMesh::loadWeights(const string _Path)
{
	Skeleton::BoneList& b = _skeleton->getBoneList();

	_bone_index_array.resize(_vertex_array.size());
	_bone_weight_array.resize(_vertex_array.size());


	vector<vector<SkinIndexWeight > > temp_weights;
	temp_weights.resize(_vertex_array.size());


    // *** initialise weights to 0 ***
	for(int i=0; i<_vertex_array.size(); ++i)
	{
		temp_weights[i].resize(b.size());
        for(int j=0; j<b.size(); ++j)
        {
            temp_weights[i][j].first = j;
            temp_weights[i][j].second = 0.f;
        }
    }

    for(unsigned int i=0; i<b.size(); ++i)
    {
        Bone* current_bone = b[i];
        string filename = _Path + current_bone->getName() + string(".txt");
        cout << filename << endl;

        ifstream weightfile;
        weightfile.open(filename.c_str());
        if(weightfile.is_open())
        {
            while(!weightfile.eof())
            {
                int index;
                float weight;
                weightfile >> index >> weight;
                if(index<temp_weights.size())
                {
                	for(int j=0; j<b.size(); ++j) temp_weights[index][j].second *= 1.f-weight; // *** perform normalisation at this time ***
					temp_weights[index][i].second = weight;
                }
            }
        }
        weightfile.close();
    }

	int max_weights = 4; if(max_weights>b.size()) max_weights = b.size();
	// *** sort and crop ***
	for(int i=0; i<_vertex_array.size(); ++i)
	{
		sort(temp_weights[i].begin(), temp_weights[i].end(), SortSkinIndexWeightFunctor() );
		temp_weights[i].erase(temp_weights[i].begin()+max_weights,temp_weights[i].end()); // only keep highest 4
		float sum = temp_weights[i][0].second + temp_weights[i][1].second + temp_weights[i][2].second + temp_weights[i][3].second; // normalise weights
		_bone_index_array[i] = v4(temp_weights[i][0].first, temp_weights[i][1].first, temp_weights[i][2].first, temp_weights[i][3].first);
		_bone_weight_array[i] = v4(temp_weights[i][0].second, temp_weights[i][1].second, temp_weights[i][2].second, temp_weights[i][3].second)/sum;
	}

}

} // namespace dalelib
